#include <iostream>
#include "implicitSOR.h"

ImplicitSOR::ImplicitSOR() : IntegrationMethod()
{
}

ImplicitSOR::ImplicitSOR(double _deltaT, double _g, std::vector<State> _s0,
                         std::vector< std::vector<Particle *> > &_p, std::vector< std::vector<Spring *> > &_s,
                         bool _activeAir, int _airType, double _airPeriod, double _airRepeat, double _airViscosity, Vector3D _airDirection)
                         : IntegrationMethod(_deltaT, _g, _s0, _p, _s, _activeAir, _airType, _airPeriod, _airRepeat, _airViscosity, _airDirection)
{
    threshold = 0.2;
    relaxation = 1.0;
    maxIteration = 100;
}

ImplicitSOR::~ImplicitSOR()
{

}
void ImplicitSOR::calculate(bool, unsigned int _fabricPosition, double _fraction)
{
    State s = currentState.at(_fabricPosition); //current state

    double t = s.getT(); //time

    unsigned int dof = s.getDOF(); //degree of freedom

    Matrix reference = s.getReferenceMatrix(); //matrix reference

    std::vector< std::pair<bool, Object *> > collision = s.getCollision();

    currentDeltaT = deltaT / _fraction;

    Vector *resultanteForce = populateResultantForces(currentState.at(_fabricPosition), _fabricPosition); //resultant force vector at time t

    //Vector *resultanteForce = populateResultantForces(currentState.at(_fabricPosition), _fabricPosition, ZHOU2005); //resultant force vector at time t

    std::vector<Particle *> particlesI = particles.at(_fabricPosition); //vector of particles

    std::vector<Spring *> springsI = springs.at(_fabricPosition); //vector of springs

    std::vector<Vector3D*> offsetVelocity; //vector that stores the displacement vectors

    std::vector<Vector3D*> deltaOffsetVelocity; //vector that stores the deltas vectors

    std::vector<Matrix*>matrixInverse; //vector containing the matrices  inversa of the equation 12 of each partícle

    std::vector<Vector3D*>force;  //vector containing as forces f~ acting on each particle

    std::vector<std::vector<Matrix*> >derive; //vector of vectors of the Jacobian matrices with respect to velocity

    std::vector<std::vector<Matrix*> >deriveP; //vector of vectors of the Jacobian matrices with respect to position

    std::vector<Vector3D*> offsetVelocityOld; ////vector that stores the displacement vectors of the previous interaction

    //State currentTemp, previousTemp; //temporary states

    //calculating the initial guess
    for(unsigned int j=0; j<particlesI.size(); j++)
    {

        if((reference[j][0] == -1) && (reference[j][1] == -1) && (reference[j][2] == -1)) //eliminating the calculations for the particles are all trapped
        {
            offsetVelocity.push_back(new Vector3D);

            deltaOffsetVelocity.push_back(new Vector3D);

            matrixInverse.push_back(new Matrix);

            force.push_back(new Vector3D);

            std::vector<Matrix *> aux;

            derive.push_back(aux);

            deriveP.push_back(aux);

            offsetVelocityOld.push_back(new Vector3D);

            continue;
        }

        Vector3D currentVelocity, currentPosition, currentForce3D; //position, speed and force vectors of the particle J in the current state

        s.getParticlePositionVelocity(j, particlesI.at(j)->getPosition(), particlesI.at(j)->getVelocity(), &currentPosition, &currentVelocity);

        int place;

        for(unsigned int k=0; k<3; k++)
        {
            place = reference[j][k];
            if (place != -1)
            {
            currentForce3D[k] = (*resultanteForce)[place];

            }
        }

        deltaOffsetVelocity.push_back(new Vector3D);

        Matrix m = populateMatrixMass(s, _fabricPosition, j); //mass matrix of the particle


        std::vector<Matrix*> derivedPosition;
        derivedPosition = vectorOfJacobianPosition(s, currentPosition, currentVelocity, _fabricPosition, j); // vector of Jacobin matrices (position) of the particle J
        deriveP.push_back(derivedPosition);


        std::vector<Matrix*> derivedVelocity;
        derivedVelocity = vectorOfJacobianVelocity(s, currentPosition, _fabricPosition, j); // vector of Jacobin matrices (velocity) of the particle J
        derive.push_back(derivedVelocity);


        Matrix jacobianPos = derivatedForcePositionItself(derivedPosition, _fabricPosition, j); //Jacobian matrix with respect to the position of the particle J

        Matrix jacobianVel = derivativeForceVelocityItself(derivedVelocity, _fabricPosition, j); //Jacobian matrix with respect to the velocity of the particle J


        Matrix term = (jacobianPos)*(currentDeltaT*currentDeltaT);
        Matrix term1 = (jacobianVel)*currentDeltaT;

        //derived with respect the air force
        Matrix ident (3,3, IDENTITY_MATRIX);
        Matrix term2 = ident * ((particlesI.at(j)->getKd())*(-1.0));

        Matrix w = ((m - term) - term1) - term2;

        if(useAir)
        {

            Particle *particle = particlesI.at(j);

            Vector3D normal = s.getParticleNormal(particle);

            w = w - ((Vector(normal).outterProduct(Vector(normal)))*(fluidViscosity * (-1.0)));

        }

        Matrix *inverse = new Matrix ((w).inverse());
        matrixInverse.push_back(inverse);


        Vector3D *forces = new Vector3D (forceResultantParticle(s, currentForce3D, currentVelocity, derivedPosition, _fabricPosition, j)); //force f~
        force.push_back(forces);

        Vector3D *deltaV0 = new Vector3D((*inverse)*(Vector((*forces) * currentDeltaT))); //initial guess
        offsetVelocity.push_back(deltaV0);

        offsetVelocityOld.push_back(new Vector3D);


        //calculateImplicitSOR(&s, currentPosition, currentVelocity, *deltaV0, j);


    }

    //previousTemp = currentState.at(_fabricPosition);
    //currentTemp = s;
    //currentTemp.setT_DOF_Ref_Col(t + currentDeltaT, dof, reference, collision);


    bool converge = false; //variable convergence

    int iteration = 0; //number of iteraction

    do
    {
        iteration++;

        double max = -1;

        for(unsigned int j=0; j<particlesI.size(); j++)
        {
            if((reference[j][0] == -1) && (reference[j][1] == -1) && (reference[j][2] == -1)) continue; //eliminating the calculations for the particles are all trapped


            Vector3D currentVelocity, currentPosition; //position and speed vectors of the particle J in the current state

            currentState.at(_fabricPosition).getParticlePositionVelocity(j, particlesI.at(j)->getPosition(), particlesI.at(j)->getVelocity(), &currentPosition, &currentVelocity);

            std::vector<Matrix*> derivedPosition = deriveP.at(j); //vector of Jacobin matrices (position) of the particle J

            std::vector<Matrix *> derivedVelocity = derive.at(j); //vector of Jacobin matrices (velocity) of the particle J

            Matrix jacobianPos = derivatedForcePositionItself(derivedPosition, _fabricPosition, j); //Jacobian matrix with respect to the position of the particle J

            Matrix jacobianVel = derivativeForceVelocityItself(derivedVelocity, _fabricPosition, j); //Jacobian matrix with respect to the velocity of the particle J

            Matrix *inverse = matrixInverse.at(j); //inverse matriz of the equation 12

            Vector3D *forces = force.at(j); // force f~

            unsigned int size = springsI.size(); //size of vector of springs

            Vector3D result;

            //calculating the sum of equation 12
            for(unsigned int i = 0; i < size; i++)
            {
                Spring *s = springsI.at(i);

                if (!((s->getParticleBegin() == (int)j) || (s->getParticleEnd() == (int)j))) continue;

                Matrix jacobPos = (*derivedPosition[i]) *(currentDeltaT*currentDeltaT); //jacobian position

                Matrix jacobVel = (*derivedVelocity[i])*(currentDeltaT); //jacobian velocity

                Vector3D j_;

                if(s->getParticleBegin() == (int)j)
                {
                   j_ = (*offsetVelocity[s->getParticleEnd()]); //the speed of the particle connected to particle J
                }
                else
                {
                   j_ = (*offsetVelocity[s->getParticleBegin()]); //the speed of the particle connected to particle J
                }

                Matrix s_ = (jacobPos+jacobVel); //equation 12 (forth line)

                result = result + Vector3D(s_*Vector(j_));

            }
            //result = result + (((jacobianPos*(currentDeltaT*currentDeltaT))+(jacobianVel*currentDeltaT))*(*offsetVelocity[j]));

            //calculando the equation 12
            Matrix inv = (*inverse)*relaxation;

            Vector3D f = (*forces)*currentDeltaT;

            Vector3D t = result*(currentDeltaT*currentDeltaT);

            Vector3D fr = f + t;

            (*deltaOffsetVelocity.at(j)) = Vector3D(inv*Vector(fr));

            offsetVelocityOld.at(j) = new Vector3D(*offsetVelocity.at(j));

            (*offsetVelocity.at(j)) = Vector3D ( (*offsetVelocity.at(j)) + (*deltaOffsetVelocity.at(j)) );

            double norm = ((*offsetVelocity.at(j)) - (*offsetVelocityOld.at(j))).length();

            if(norm > max)
            {
                max = norm;
            }

             //Vector3D position, velocity;  // position and velocity in the temporary current state

             //currentTemp.getParticlePositionVelocity(j, particlesI.at(j)->getPosition(), particlesI.at(j)->getVelocity(), &position, &velocity); //calculating the new temporary state of the current interaction

             //calculateImplicitSOR(&s, position, velocity, (*offsetVelocity.at(j)), j);

        }

        if (max < threshold)
        {
            converge = true;
        }

        //updating the temporary states

        /*previousTemp = currentTemp;

        currentTemp = s;

        currentTemp.setT_DOF_Ref_Col(t + currentDeltaT, dof, reference, collision);

        //calculating the error

        Vector dirP = (currentTemp.getPosition()) - (previousTemp.getPosition());

        double normP = dirP.length();

        if(normP < threshold)
        {
            converge = true;

        }*/
    }while (!converge && iteration < maxIteration);

    for(unsigned int j=0; j<particlesI.size(); j++)
    {
        Vector3D currentVelocity, currentPosition; //position and speed vectors of the particle J in the current state

        currentState.at(_fabricPosition).getParticlePositionVelocity(j, particlesI.at(j)->getPosition(), particlesI.at(j)->getVelocity(), &currentPosition, &currentVelocity);

        calculateImplicitSOR(&s, currentPosition, currentVelocity, (*offsetVelocity.at(j)), j);


    }


    //updating the state after the process to converge

    previousState.at(_fabricPosition) = currentState.at(_fabricPosition);

    currentState.at(_fabricPosition) = s;

    currentState.at(_fabricPosition).setT_DOF_Ref_Col(t + currentDeltaT, dof, reference, collision);

    delete resultanteForce;


    for(unsigned int i = 0; i < particlesI.size(); i++)
    {
        Vector3D *aux = offsetVelocity.at(i);
        delete aux;

        aux = deltaOffsetVelocity.at(i);
        delete aux;

        aux = offsetVelocityOld.at(i);
        delete aux;

        if((reference[i][0] == -1) && (reference[i][1] == -1) && (reference[i][2] == -1)) continue;

        else
        {
            std::vector<Matrix*> derivedPosition = deriveP.at(i);

            std::vector<Matrix *> derivedVelocity = derive.at(i);

            for(unsigned int l = 0; l < springsI.size(); l++)
            {
                Matrix *aux = derivedPosition.at(l);
                delete aux;

                aux = derivedVelocity.at(l);
                delete aux;

            }

        }

    }

}

//returns the mass matrix of a given particle
Matrix ImplicitSOR::populateMatrixMass(const State &_state, unsigned int _i, unsigned int _j)
{

    const Matrix &ref = _state.getReferenceMatrix();

    std::vector<Particle *> particlesI = particles.at(_i);

    Matrix m = Matrix(3, 3);

    int place;

    for(unsigned int k=0; k<3; k++)
    {
        place = ref[_j][k];

        if (place != -1)
        {
          m[k][k] = particlesI.at(_j)->getMass();
        }

    }

    return m;
}

//calculate the Jacobian relating the position
Matrix *ImplicitSOR::derivativeForcePosition(const State& _state, Spring *_s, const Vector3D &_positionBegin, const Vector3D &_positionEnd, const Vector3D &_velocityBegin, const Vector3D &_velocityEnd, unsigned int _j)
{

    const Matrix &ref = _state.getReferenceMatrix();

    double delta, ks, kd, lo, lc;

    delta = _s->positiveDelta(); //tells you if the spring is stretched or comp

    ks = _s->getKs(); //stiffness coefficient

    kd = _s->getKd(); //damping coefficient

    lo = _s->getOriginalLength(); //initial length of the spring

    lc = _s->getCurrentLength(_positionBegin, _positionEnd); //current length of the spring

    Matrix ident(3,3, IDENTITY_MATRIX);

    Matrix *result;

    if((_s->getType() == FLEXION && !delta) || (_s->getType() != FLEXION && delta))
    {
        //derivative with respect to the spring force

        Vector xij = Vector(_positionEnd - _positionBegin);

        Matrix temp1 =  (xij.outterProduct(xij));

        double term = (ks * (lc - lo)) / lc;

        double lengthSquared = lc * lc;

        double term1 = (ks*lo)/(lc*lengthSquared);

        //including derivative with respect to the damping force

        Vector vij = Vector(_velocityEnd - _velocityBegin);

        Matrix temp2 =  (vij.outterProduct(xij)) * (1.0 / lengthSquared);

        Matrix temp3 = ident * ((vij.innerProduct(xij)) * (1.0 / lengthSquared));

        Matrix temp4 = temp1 * (((vij.innerProduct(xij)) * 2.0) / (lengthSquared * lengthSquared));

        //result = new Matrix((ident * term) + (temp1 * term1));

        result = new Matrix(((ident * term) + (temp1 * term1)) + ((temp2 - temp4 + temp3) * kd));
    }
    /*else if(_s->getType()!=FLEXION && !delta)
    {

        Vector xij = Vector(_positionEnd - _positionBegin);

        Matrix temp1 =(xij.outterProduct(xij));

        double lengthSquared = lc * lc;

        //including derivative with respect to the damping force

        Vector vij = Vector(_velocityEnd - _velocityBegin);

        Matrix temp2 =  (vij.outterProduct(xij)) * (1.0 / lengthSquared);

        Matrix temp3 = ident * ((vij.innerProduct(xij)) * (1.0 / lengthSquared));

        Matrix temp4 = temp1 * (((vij.innerProduct(xij)) * 2.0) / (lengthSquared * lengthSquared));

        //result = new Matrix(temp1 * (ks/lengthSquared));

        result = new Matrix((temp1 * (ks/lengthSquared)) +((temp2 - temp4 + temp3) * kd));

    }*/
    else
    {
        result = new Matrix(3,3);
    }

    int place;

    for(unsigned int k=0; k<3; k++)
    {
        place = ref[_j][k];

        if (place == -1)
        {
            for(unsigned int i=0; i<3; i++)
            {
                (*result)[k][i] = 0;
            }

        }

    }

    return result;
}

//returns an array containing all the Jacobian matrices related to a given particle
std::vector<Matrix*> ImplicitSOR::vectorOfJacobianPosition(const State &_state, const Vector3D &_position, const Vector3D &_velocity, unsigned int _i, unsigned int _j)
{
    std::vector<Particle *> particlesI = particles.at(_i);

    std::vector<Spring *> springsI = springs.at(_i);

    unsigned int size = springsI.size();

    std::vector<Matrix*> jacobian;

    for(unsigned int i = 0; i < size; i++)
    {
        Spring *s = NULL;

        Matrix *jacobianPosition;

        Vector3D positionBegin, positionEnd, velocityBegin, velocityEnd;


        s = springsI.at(i);

        if(!(s->getParticleBegin() == (int)_j || s->getParticleEnd() == (int)_j))
        {
            jacobian.push_back(NULL);
            continue;
        }

        if(s->getParticleBegin() == (int)_j)
        {
            positionBegin = _position;

            velocityBegin = _velocity;

            positionEnd = _state.getParticlePosition(s->getParticleEnd(), particlesI.at(s->getParticleEnd())->getPosition());

            velocityEnd = _state.getParticleVelocity(s->getParticleEnd(), particlesI.at(s->getParticleEnd())->getVelocity());

            jacobianPosition = derivativeForcePosition(_state, s, positionBegin, positionEnd, velocityBegin, velocityEnd, _j);

        }
        else
        {
            positionEnd = _position;

            velocityEnd = _velocity;

            positionBegin = _state.getParticlePosition(s->getParticleBegin(), particlesI.at(s->getParticleBegin())->getPosition());

            velocityBegin = _state.getParticleVelocity(s->getParticleBegin(), particlesI.at(s->getParticleBegin())->getVelocity());

            jacobianPosition = derivativeForcePosition(_state, s, positionBegin, positionEnd, velocityBegin, velocityEnd, _j);
        }
        jacobian.push_back(jacobianPosition);

     }

    return jacobian;

}
//calculates the Jacobian matrix with respect to the position of the particle itself
Matrix ImplicitSOR::derivatedForcePositionItself(const std::vector<Matrix*> &_jacobian, unsigned int _i, unsigned int _j)
{

    std::vector<Spring *> springsI = springs.at(_i);

    unsigned int size = springsI.size();

    Matrix result(3,3);

     for(unsigned int i = 0; i < size; i++)
     {
        Spring *s = NULL;

        s = springsI.at(i);

        if(!(s->getParticleBegin() == (int)_j || s->getParticleEnd() == (int)_j)) continue;

        else
        {
            result = result + ((*_jacobian[i])*(-1.0));
        }

    }

    return result;
}

//calculating the force f~
Vector3D ImplicitSOR::forceResultantParticle(const State& _state, const Vector3D &_currentForce, const Vector3D &_velocity, const std::vector<Matrix*> &_jacobian, unsigned int _i, unsigned int _j)
{
    std::vector<Particle *> particlesI = particles.at(_i);

    std::vector<Spring *> springsI = springs.at(_i);


    unsigned int size = springsI.size();

    Vector3D result, derived;

    for(unsigned int i = 0; i < size; i++)
    {
        Spring *s = NULL;
        Vector3D velocity;

        s = springsI.at(i);

        if(s->getParticleBegin() == (int)_j)
        {
            velocity = _state.getParticleVelocity(s->getParticleEnd(), particlesI.at(s->getParticleEnd())->getVelocity());

            Matrix *m = _jacobian.at(i);

            derived = derived + Vector3D((*m) * Vector(velocity - _velocity));

        }
        else if(s->getParticleEnd() == (int)_j)
        {
            velocity = _state.getParticleVelocity(s->getParticleBegin(), particlesI.at(s->getParticleBegin())->getVelocity());

            Matrix *m = _jacobian.at(i);

            derived = derived + Vector3D((*m) * Vector(velocity - _velocity));

        }
    }
    result = _currentForce + (derived * currentDeltaT);

    return result;
}

/*Matrix *ImplicitSOR::derivativeForceVelocity(const State& _state, Spring *_s, unsigned int _j)
{

    const Matrix &ref = _state.getReferenceMatrix();

    double kd = _s->getKd();

    Matrix ident(3,3, IDENTITY_MATRIX);

    double delta;

    delta = _s->positiveDelta();

    Matrix *result;

    if((_s->getType() == FLEXION && !delta) || (_s->getType() != FLEXION ))
    {
         result = new Matrix(ident*kd);
    }
    else
    {
        result = new Matrix(3,3);
    }

    int place;

    for(unsigned int k=0; k<3; k++)
    {
        place = ref[_j][k];

        if (place == -1)
        {
          for(unsigned int i=0; i<3; i++)
          {
               (*result)[k][i] = 0;

          }

        }

    }

    return result;
}*/

//calculate the Jacobian relating the velocity
Matrix *ImplicitSOR::derivativeForceVelocity(const State& _state, Spring *_s, const Vector3D &_positionBegin, const Vector3D &_positionEnd, unsigned int _j)
{

    const Matrix &ref = _state.getReferenceMatrix();

    double kd = _s->getKd();

    double delta;

    delta = _s->positiveDelta();

    double lc = _s->getCurrentLength(_positionBegin, _positionEnd);

    Matrix *result;

    //derivative with respect to the damping force
    if((_s->getType() == FLEXION && !delta) || (_s->getType() != FLEXION && delta))
    {
        Vector xij = Vector(_positionEnd - _positionBegin);

        Matrix temp1 =  (xij.outterProduct(xij));

        double lengthSquared = lc * lc;

        result = new Matrix (temp1 * (kd / lengthSquared));

    }

    /*if((_s->getType() == FLEXION && !delta) || (_s->getType() != FLEXION))
    {
        Vector xij = Vector(_positionEnd - _positionBegin);

        Matrix temp1 =  (xij.outterProduct(xij));

        double lengthSquared = lc * lc;

        result = new Matrix (temp1 * (kd / lengthSquared));

    }*/
    else
    {
        result = new Matrix(3,3);
    }

    int place;

    for(unsigned int k=0; k<3; k++)
    {
        place = ref[_j][k];

        if (place == -1)
        {
          for(unsigned int i=0; i<3; i++)
          {
               (*result)[k][i] = 0;

          }

        }

    }

    return result;
}

/*std::vector<Matrix*> ImplicitSOR::vectorOfJacobianVelocity(const State &_state, unsigned int _i, unsigned int _j)
{
    std::vector<Particle *> particlesI = particles.at(_i);

    std::vector<Spring *> springsI = springs.at(_i);

    unsigned int size = springsI.size();

    std::vector<Matrix*> jacobian;

    for(unsigned int i = 0; i < size; i++)
    {
        Spring *s = NULL;

        s = springsI.at(i);

        if(!(s->getParticleBegin() == (int)_j || s->getParticleEnd() == (int)_j))
        {
            jacobian.push_back(NULL);
            continue;
        }
        else
        {
            jacobian.push_back(derivativeForceVelocity(_state, s, _j));
        }

    }

    return jacobian;

}*/

//returns an array containing all the Jacobian matrices related to a given particle
std::vector<Matrix*> ImplicitSOR::vectorOfJacobianVelocity(const State &_state, const Vector3D &_position, unsigned int _i, unsigned int _j)
{
    std::vector<Particle *> particlesI = particles.at(_i);

    std::vector<Spring *> springsI = springs.at(_i);

    unsigned int size = springsI.size();

    std::vector<Matrix*> jacobian;

    for(unsigned int i = 0; i < size; i++)
    {
        Spring *s = NULL;

        Matrix *jacobianVelocity;

        Vector3D positionBegin, positionEnd;

        s = springsI.at(i);

        if(!(s->getParticleBegin() == (int)_j || s->getParticleEnd() == (int)_j))
        {
            jacobian.push_back(NULL);
            continue;
        }

        if(s->getParticleBegin() == (int)_j)
        {
            positionBegin = _position;

            positionEnd = _state.getParticlePosition(s->getParticleEnd(), particlesI.at(s->getParticleEnd())->getPosition());

            jacobianVelocity = derivativeForceVelocity(_state, s, positionBegin, positionEnd, _j);
        }
        else
        {
            positionEnd = _position;

            positionBegin = _state.getParticlePosition(s->getParticleBegin(), particlesI.at(s->getParticleBegin())->getPosition());

            jacobianVelocity = derivativeForceVelocity(_state, s, positionBegin, positionEnd, _j);
        }
        jacobian.push_back(jacobianVelocity);

    }

    return jacobian;

}

//calculates the Jacobian matrix with respect to the velocity of the particle itself
Matrix ImplicitSOR::derivativeForceVelocityItself(const std::vector<Matrix*> &_jacobianVelocity, unsigned int _i, unsigned int _j)
{

    std::vector<Spring *> springsI = springs.at(_i);

    unsigned int size = springsI.size();

    Matrix result(3,3);

    for(unsigned int i = 0; i < size; i++)
    {
        Spring *s = NULL;

        s = springsI.at(i);

        if(!(s->getParticleBegin() == (int)_j || s->getParticleEnd() == (int)_j)) continue;

        else
        {
             Matrix *m = _jacobianVelocity.at(i);
             result = result + ((*m)*(-1.0));

        }

     }

    return result;

 }

 //calculate the new position and speed
 void ImplicitSOR::calculateImplicitSOR(State *_state, const Vector3D &_position, const Vector3D &_velocity,const Vector3D & _offsetVelocity, unsigned int _j)
 {

    const Matrix &ref = _state->getReferenceMatrix();

    Vector3D velocity = _velocity + (_offsetVelocity);
    Vector3D position = _position + (velocity *currentDeltaT);



    int place;
    for(unsigned int k=0; k<3; k++){
        place = ref[_j][k];

        if (place != -1)
        {
            _state->getPosition()[place] = position[k];
            _state->getVelocity()[place] = velocity[k];
        }
    }


 }


